﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using TinyROS.Exceptions;
using TinyROS.Messages;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
using System.Threading;

namespace TinyROS
{
    public class PlannerNode : Node, ICommandInteractive
    {
        private byte[,] mapCache;
        private CarStatus statusCache;
        private Coordinate3D goalCache;
        private AStarPointList pathCache;
        private Subscriber<MapMessage> MapSubscriber;
        private Subscriber<CarStatusMessage> CarStatusSubscriber;
        private Subscriber<Vector3DMessage> GoalSubscriber;
        private Subscriber<StringMessage> CommandSubscriber;
        private Publisher<DiffcarCourseMessage> VelCmdPublisher;
        private Dictionary<Publisher, string> pubedTopicName = new();
        private Dictionary<Subscriber, string> subedTopicName = new();
        private CancellationTokenSource PathExeCanceller;
        public override Dictionary<Publisher, string> PublishedTopicName => this.pubedTopicName;
        public override Dictionary<Subscriber, string> SubscribedTopicName => this.subedTopicName;
        private bool running = false;
        public override bool IsRunning => this.running;

        public PlannerNode() : this(
            TinyROSConfig.DefaultTopicName.DiffcarCommandMuxIn2,
            TinyROSConfig.DefaultTopicName.Map,
            TinyROSConfig.DefaultTopicName.EstimatedCarStaus,
            TinyROSConfig.DefaultTopicName.Goal,
            TinyROSConfig.DefaultTopicName.NodeCommand.Planner)
        {

        }
        public PlannerNode(string velPubName, string mapSubName, string statusSubName, string goalSubName, string cmdSubName)
        {
            this.VelCmdPublisher = new();
            this.PublishedTopicName.Add(this.VelCmdPublisher, velPubName);
            this.MapSubscriber = new((MapMessage msg) => { this.mapCache = msg.Value.Global; this.pathCache = null; });
            this.SubscribedTopicName.Add(this.MapSubscriber, mapSubName);
            this.CarStatusSubscriber = new((CarStatusMessage msg) => { this.statusCache = msg.Value; this.pathCache = null; });
            this.SubscribedTopicName.Add(this.CarStatusSubscriber, statusSubName);
            this.GoalSubscriber = new((Vector3DMessage msg) => { this.goalCache = msg.Value; this.pathCache = null; });
            this.SubscribedTopicName.Add(this.GoalSubscriber, goalSubName);
            this.CommandSubscriber = new((StringMessage msg) => { this.ExecuteCommandMute(msg.Value); });
            this.SubscribedTopicName.Add(this.CommandSubscriber, cmdSubName);
        }


        public override void Init(SystemCore master)
        {
            var velPubTopic = master.RequestTopic<DiffcarCourseTopic>(this.PublishedTopicName[this.VelCmdPublisher]);
            this.VelCmdPublisher.SetTopic(velPubTopic);
            var mapSubTopic = master.RequestTopic<MapTopic>(this.SubscribedTopicName[this.MapSubscriber]);
            this.MapSubscriber.SetTopic(mapSubTopic);
            var statusSubTopic = master.RequestTopic<CarStatusTopic>(this.SubscribedTopicName[this.CarStatusSubscriber]);
            this.CarStatusSubscriber.SetTopic(statusSubTopic);
            var goalSubTopic = master.RequestTopic<Vector3DTopic>(this.SubscribedTopicName[this.GoalSubscriber]);
            this.GoalSubscriber.SetTopic(goalSubTopic);
            var cmdSubTopic = master.RequestTopic<CommandTopic>(this.SubscribedTopicName[this.CommandSubscriber]);
            this.CommandSubscriber.SetTopic(cmdSubTopic);
            this.Initialized = true;
        }

        public override void Run()
        {
            if(this.Initialized)
            {
                this.MapSubscriber.Start(Subscriber<MapMessage>.SubscribeType.Passive);
                this.CarStatusSubscriber.Start(Subscriber<CarStatusMessage>.SubscribeType.Passive);
                this.GoalSubscriber.Start(Subscriber<Vector3DMessage>.SubscribeType.Passive);
                this.CommandSubscriber.Start(Subscriber<StringMessage>.SubscribeType.Passive);
                this.running = true;
            }
            else
            {
                throw new NodeNotInitedException();
            }
        }

        public override void Stop()
        {
            this.MapSubscriber.Stop();
            this.CarStatusSubscriber.Stop();
            this.GoalSubscriber.Stop();
            this.CarStatusSubscriber.Stop();
            this.running = false;
        }
        public void ExecuteCommand(string cmd, bool raiseError = true)
        {
            try
            {
                (var type, var args) = this.ParseCommand(cmd);
                switch (type)
                {
                    case CommandType.CalculatePath:
                        SystemCore.ConsoleWriteLineWithColor("Planner: begin planning path", ConsoleColor.White);
                        this.CalculatePath((int)args[0]);
                        SystemCore.ConsoleWriteLineWithColor("Planner: planning path done", ConsoleColor.Green);
                        break;
                    case CommandType.PerformPath:
                        SystemCore.ConsoleWriteLineWithColor("Planner: begin executing path", ConsoleColor.White);
                        try
                        {
                            this.PathExeCanceller = new();
                            this.ExecutePathOpenLoopAsync((float)args[0], this.PathExeCanceller.Token);
                        }catch(OperationCanceledException)
                        {
                            SystemCore.ConsoleWriteLineWithColor("Planner: executing path cancelled", ConsoleColor.Yellow);
                        }                       
                        //SystemCore.ConsoleWriteLineWithColor("Planner: executing path done", ConsoleColor.Green);
                        break;
                    case CommandType.StopPerformPath:
                        try
                        {
                            this.PathExeCanceller.Cancel();
                        }catch
                        {

                        }
                        break;
                    case CommandType.PrintPath:
                        this.PrintPath();
                        break;
                    case CommandType.PrintMap:
                        this.PrintMap();
                        break;
                }
            }
            catch(Exception e)
            {
                if (raiseError) { throw; }
                else
                {
                    SystemCore.ConsoleWriteLineWithColor(e.StackTrace, ConsoleColor.White);
                }
            }
        }

        public void ExecuteCommandMute(string cmd, bool raiseError = true)
        {
            try
            {
                (var type, var args) = this.ParseCommand(cmd);
                switch(type)
                {
                    case CommandType.CalculatePath:
                        this.CalculatePath((int)args[0]);
                        break;
                    case CommandType.PerformPath:
                        try
                        {
                            this.PathExeCanceller = new();
                            this.ExecutePathOpenLoopAsync((float)args[0], this.PathExeCanceller.Token);
                        }
                        catch (OperationCanceledException)
                        {
                        }
                        //SystemCore.ConsoleWriteLineWithColor("Planner: executing path done", ConsoleColor.Green);
                        break;
                    case CommandType.StopPerformPath:
                        try
                        {
                            this.PathExeCanceller.Cancel();
                        }
                        catch
                        {

                        }
                        break;
                    case CommandType.PrintPath:
                        this.PrintPath();
                        break;
                    case CommandType.PrintMap:
                        this.PrintMap();
                        break;
                }
            }
            catch(PathNotFoundException)
            {
                if (raiseError) { throw; }
                else
                {
                    SystemCore.ConsoleWriteLineWithColor("planner: could not find a path", ConsoleColor.Red);
                }
            }
            catch
            {
                if (raiseError) { throw; }
            }
            
        }
        private enum CommandType { CalculatePath, PerformPath, StopPerformPath, PrintPath, PrintMap}
        private (CommandType type, object[] args) ParseCommand(string cmd)
        {
            try
            {
                var s = cmd.Split(" ", StringSplitOptions.RemoveEmptyEntries);
                var c = s[0];
                if (c == "getpath")
                {
                    string r;
                    try
                    {
                        r = s[1];
                    }
                    catch(IndexOutOfRangeException)
                    {
                        return (CommandType.CalculatePath, new object[] { 5 });
                    }                   
                    try
                    {
                        r = s[1];
                        var rn = int.Parse(r);
                        if (rn <= 0)
                        {
                            throw new Exception();
                        }
                        return (CommandType.CalculatePath, new object[] {rn});
                    }
                    catch
                    {
                        throw new InvalidArgumentException(cmd, r);
                    }
                    
                }
                if (c == "execute")
                {
                    string linear;
                    try
                    {
                        linear = s[1];
                    }
                    catch (IndexOutOfRangeException)
                    {
                        return (CommandType.PerformPath, new object[] { 0.1f });
                    }
                    try
                    {
                        linear = s[1];
                        var linearF = float.Parse(linear);
                        if (linearF <= 0)
                        {
                            throw new Exception();
                        }
                        return (CommandType.PerformPath, new object[] { linearF });
                    }
                    catch
                    {
                        throw new InvalidArgumentException(cmd, linear);
                    }                    
                }
                if (c == "stop")
                {
                    return (CommandType.StopPerformPath, null);
                }
                if (c == "printpath")
                {
                    return (CommandType.PrintPath, null);
                }
                if (c == "printmap")
                {
                    return (CommandType.PrintMap, null);
                }
            }
            catch(IndexOutOfRangeException)
            {
                throw new ArgumentCountException(cmd);
            }
            catch(InvalidArgumentException ae)
            {
                throw ae;
            }
            throw new CommandNotFoundException(cmd);
        }
        private void CalculatePath(int radius)
        {
            if(this.mapCache == null)
            {
                throw new NotEnoughForPlanningException("there is no map");
            }
            var map = new Map(this.mapCache);
            if(this.goalCache == null)
            {
                throw new NotEnoughForPlanningException("there is no goal");
            }
            var goal = new Point((int)this.goalCache.X, (int)this.goalCache.Y);
            if(this.statusCache == null)
            {
                throw new NotEnoughForPlanningException("this is no current position");
            }
            var start = new Point((int)this.statusCache.Position.X, (int)this.statusCache.Position.Y);
            var planner = new AStar(radius);
            var path = planner.GetPath(map, start, goal);
            if(path == null)
            {
                throw new PathNotFoundException();
            }
            this.pathCache = path;
        }
        private async void ExecutePathOpenLoopAsync(float linear, CancellationToken token)
        {
            try
            {
                if (this.pathCache == null)
                {
                    throw new PathNotFoundException();
                }
                var x = (int)this.statusCache.Position.X;
                var y = (int)this.statusCache.Position.Y;
                var path = this.pathCache;
                float angle = 0.0f;
                // const float linear = 0.1f;
                for (int i = 0; i < path.Count - 1; i++)
                {
                    var xNext = path[i].X;
                    var yNext = path[i].Y;
                    float dx = (xNext - x);
                    float dy = (yNext - y);
                    x = xNext;
                    y = yNext;
                    float newAngle = MathF.Atan2(dy, dx);
                    float diffAngle = newAngle - angle;
                    const float angleLinear = 0.1f;
                    const int period = 1500;
                    if (MathF.Abs(diffAngle) >= 0.00001)
                    {
                        angle = newAngle;
                        int side = -MathF.Sign(diffAngle);
                        float tTurn = MathF.Abs(diffAngle) / (angleLinear / 0.1f);
                        var turnCourse = new DiffcarCourse(angleLinear, side * 0.00001f);
                        int tTurnIntMs = (int)(tTurn * 1000);
                        int turnP = tTurnIntMs / period;
                        int turnM = tTurnIntMs % period;
                        DiffcarCourseMessage turnMsg;
                        for (int j = 0; j < turnP + 1; j++)
                        {
                            turnMsg = new DiffcarCourseMessage(turnCourse, this);
                            this.VelCmdPublisher.Talk(turnMsg);
                            await Task.Delay(period, token);
                        }
                        turnMsg = new DiffcarCourseMessage(turnCourse, this);
                        this.VelCmdPublisher.Talk(turnMsg);
                        await Task.Delay(turnM, token);
                    }
                    else
                    {

                    }

                    float dist = MathF.Sqrt(dx * dx + dy * dy) * 0.1f;
                    float tGo = dist / linear;
                    var goCourse = new DiffcarCourse(linear, 10000.0f);
                    int tGoIntMs = (int)(tGo * 1000);
                    int goP = tGoIntMs / period;
                    int goM = tGoIntMs % period;
                    DiffcarCourseMessage goMsg;
                    for (int j = 0; j < goP; j++)
                    {
                        goMsg = new DiffcarCourseMessage(goCourse, this);
                        this.VelCmdPublisher.Talk(goMsg);
                        await Task.Delay(period, token);
                    }
                    goMsg = new DiffcarCourseMessage(goCourse, this);
                    this.VelCmdPublisher.Talk(goMsg);
                    await Task.Delay(goM, token);

                }
            }
            catch(TaskCanceledException)
            {
                //throw new OperationCanceledException();
                SystemCore.ConsoleWriteLineWithColor("Planner: executing cancelled", ConsoleColor.Yellow);
            }
        }
        private void PrintMap()
        {
            if (this.mapCache == null)
            {
                throw new NotEnoughForPrintException();
            }           
            int h = this.mapCache.GetLength(0);
            int w = this.mapCache.GetLength(1);
            for (int x = 0; x < h; x++)
            {
                for (int y = 0; y < w; y++)
                {
                    if (this.mapCache[x, y] == 0)
                    {
                        SystemCore.ConsoleWriteWithColor("o", ConsoleColor.White);
                    }
                    else
                    {
                        SystemCore.ConsoleWriteWithColor("x", ConsoleColor.Red);
                    }
                }
                SystemCore.ConsoleWriteLineWithColor("", ConsoleColor.White);
            }
        }
        private void PrintPath()
        {
            if(this.mapCache==null)
            {
                throw new NotEnoughForPrintException();
            }
            if(this.pathCache==null)
            {
                throw new NotEnoughForPrintException();
            }
            int h = this.mapCache.GetLength(0);
            int w = this.mapCache.GetLength(1);
            for(int x = 0; x < h; x++)
            {
                for (int y = 0; y < w; y++) 
                {
                    if(this.pathCache.ContainsPoint(x,y))
                    {
                        SystemCore.ConsoleWriteWithColor("s", ConsoleColor.Green);
                    }
                    else if(this.mapCache[x,y]==0)
                    {
                        SystemCore.ConsoleWriteWithColor("o", ConsoleColor.White);
                    }
                    else
                    {
                        SystemCore.ConsoleWriteWithColor("x", ConsoleColor.Red);
                    }
                }
                SystemCore.ConsoleWriteLineWithColor("", ConsoleColor.White);
            }
        }
    }
    public class AStar
    {
        private AStarPointList Open = new();
        private AStarPointList Close = new();
        public int Radius { get; set; } = 4; 
        public AStar()
        {
           
        }
        public AStar(int r)
        {
            this.Radius = r;
        }
        public AStarPointList GetPath(Map map, Point start, Point end)
        {
            // 初始时将起点放入Open
            PointAStar p0 = new PointAStar(start.X, start.Y);
            p0.G = 0.0f;
            p0.H = PointAStar.MhtDistance(p0, end);
            this.Open.Add(p0);
            PointAStar endA = new(end.X, end.Y);
            while(this.Open.Count > 0)
            {
                // 选出Open中F最小的点，移到Close中
                this.Open.SortByF();
                var p = this.Open[0];
                this.Open.Remove(p);
                this.Close.Add(p);
                // 寻找一个没有障碍的矩形区域
                int clearR = 1;
                for (int r = this.Radius; r > 1; r--) 
                {
                    if (map.IsAllClear(p, r))
                    {
                        clearR = r;
                        break;
                    }
                }
                // 若终点包含在没有障碍的区域里，则完成搜索
                if (Math.Abs(p.X - end.X) <= clearR && Math.Abs(p.Y - end.Y) <= clearR)
                {
                    endA.Father = p;
                    endA.G = endA.Father.G + PointAStar.EucDistance(p, endA);
                    endA.H = 0.0f;
                    break;
                }
                // 否则，将最外圈尝试加入Open，内部加入Close
                // 加入Open时：是障碍或者在Close里则取消；已经在Open中则根据G决定是否更新Father；加入时更新G和H并记录Father
                this.AddAroundToOpenOrClose(map, p, clearR, end);
                //throw new NotImplementedException();
            }
            if (endA.Father == null)
            {
                // 这说明没有路径能够抵达终点
                return null;
            }
            else
            {
                AStarPointList path = new();
                var pBack = endA;
                while (pBack.Father != null)
                {
                    path.Add(pBack);
                    pBack = pBack.Father;
                }
                path.Reverse();
                return path;
            }         
        }
        public async Task<AStarPointList> GetPathAsync(Map map, Point start, Point end)
        {
            var path = await Task.Run(() => { return this.GetPath(map, start, end); });
            return path;
        }
        private void AddAroundToOpenOrClose(Map map, PointAStar p, int r, Point end)
        {
            if (r == 1)
            {
                // r = 1 时，周围可能有障碍，因此逐一判断
                int x0 = p.X;
                int y0 = p.Y;
                for (int hor = -1; hor <= 1; hor++)
                {
                    for (int ver = -1; ver <= 1; ver++)
                    {
                        int x = x0 + ver;
                        int y = y0 + hor;
                        if (!map.IsInBound(x, y)) 
                        {
                            continue;
                        }
                        if (map[x, y] != 0)
                        {
                            continue;
                        }
                        if (this.Close.ContainsPoint(x, y))
                        {
                            continue;
                        }
                        this.TryAddToOpen(p, x, y, end);
                    }
                }
            }
            else
            {
                // r > 1 时，该范围一定没有障碍
                int left = Math.Max(p.Y - r, 0);
                int right = Math.Min(p.Y + r, map.Width - 1);
                int up = Math.Max(p.X - r, 0);
                int down = Math.Min(p.X + r, map.Height - 1);
                // 由left, right, up, down围成的范围一定在地图上且没有障碍
                for (int y = left + 1; y < right; y++) 
                {
                    for (int x = up + 1; x < down; x++)
                    {
                        if (!this.Close.ContainsPoint(x, y))
                        {
                            Close.Add(new PointAStar(x, y));
                        }
                    }
                }
                for (int y = left; y <= right; y++)
                {
                    int x = up;
                    this.TryAddToOpen(p, x, y, end);
                    x = down;
                    this.TryAddToOpen(p, x, y, end);
                }
                for (int x = up; x <= down; x++)
                {
                    int y = left;
                    this.TryAddToOpen(p, x, y, end);
                    y = right;
                    this.TryAddToOpen(p, x, y, end);
                }
            }
        }
        private void TryAddToOpen(PointAStar p, int x, int y, Point end)
        {
            //只针对于特定情况调用：
            //已经假设x, y在地图上且不是障碍
            if (this.Close.ContainsPoint(x, y))
            {
                return;
            }
            if (this.Open.ContainsPoint(x, y))
            {
                var pt = Open.GetPoint(x, y);
                float newG = p.G + PointAStar.EucDistance(p, pt);
                if (newG < pt.G)
                {
                    pt.G = newG;
                    pt.Father = p;
                }
            }
            else
            {
                var pn = new PointAStar(x, y);
                pn.Father = p;
                pn.G = p.G + PointAStar.EucDistance(p, pn);
                pn.H = PointAStar.EucDistance(pn, end);
                this.Open.Add(pn);
            }
        }
    }
    public class Point
    {
        public int X { get; set; }
        public int Y { get; set; }
        public Point()
        {
            this.X = 0;
            this.Y = 0;
        }
        public Point(int x,int y)
        {
            this.X = x;
            this.Y = y;
        }
    }
    public class Map
    {
        public int Height { get; }
        public int Width { get; }
        public byte[,] Value { get; }
        public byte[] PngStream
        {
            get
            {
                using var img = new Bitmap(this.Width, this.Height);
                for (var x = 0; x < this.Height; x++) 
                {
                    for (var y = 0; y < this.Width; y++)
                    {
                        if(this.Value[x, y]!=0)
                        {
                            img.SetPixel(y, x, Color.Black);
                        }
                        else
                        {
                            img.SetPixel(y, x, Color.White);
                        }
                    }
                }
                using MemoryStream stream = new();
                img.Save(stream, ImageFormat.Png);
                var l = (int)stream.Length;
                byte[] png = new byte[l];
                stream.Read(png, l, 0);
                return png;
            }
        }
        public byte this[int x,int y]
        {
            get
            {
                if (x < 0 || x >= this.Height || y < 0 || y >= this.Width) 
                {
                    throw new IndexOutOfRangeException();
                }
                else
                {
                    return this.Value[x, y];
                }
            }
            set
            {
                this.Value[x, y] = value;
            }
        }
        public Map(int height, int width)
        {
            this.Height = height;
            this.Width = width;
            this.Value = new byte[height, width];
        }
        public Map(byte[,] data)
        {
            this.Value = data;
            this.Height = data.GetLength(0);
            this.Width = data.GetLength(1);
        }
        public bool IsAllClear(Point p, int r)
        {
            int left = p.Y - r;
            int right = p.Y + r;
            int up = p.X - r;
            int down = p.X + r;
            if (left < 0) { left = 0; }
            if (right > this.Width - 1) { right = this.Width - 1; }
            if (up < 0) { up = 0; }
            if (down > this.Height - 1) { down = this.Height - 1; }
            for (int x = up; x <= down; x++)
            {
                for (int y = left; y <= right; y++)
                {
                    if (this.Value[x, y] != 0)
                    {
                        return false;
                    }
                }
            }
            return true;
        }
        public bool IsInBound(int x, int y)
        {
            return (x >= 0 && x < this.Height) && (y >= 0 && y < this.Width);
        }
        public static Map FromPng(string pngFilePath)
        {
            using var mapStream = File.OpenRead(pngFilePath);
            using Bitmap map = new Bitmap(mapStream);
            var h = map.Height;
            var w = map.Width;
            byte[,] mapMatrix = new byte[h, w];
            for (var x = 0; x < h; x++)
            {
                for (var y = 0; y < w; y++)
                {
                    var pixel = map.GetPixel(y, x).R;
                    mapMatrix[x, y] = (byte)(pixel == 255 ? 0 : 1);
                }
            }
            return new Map(mapMatrix);
        }
        public static Map EmptyMap(int height, int width)
        {
            return new Map(height, width);
        }
    }
    public class PointAStar : Point
    {
        public PointAStar Father { get; set; }
        public float G { get; set; }
        public float H { get; set; }
        public float F { get => this.G + this.H; }
        public PointAStar()
        {

        }
        public PointAStar(int x, int y)
        {
            this.X = x;
            this.Y = y;
        }
        public static int CompareByG(PointAStar p1, PointAStar p2)
        {
            float g1 = p1.G;
            float g2 = p2.G;
            float d = g1 - g2;
            if (d > 0) { return 1; }
            else if (d < 0) { return -1; }
            else { return 0; }
        }
        public static int CompareByF(PointAStar p1, PointAStar p2)
        {
            float f1 = p1.F;
            float f2 = p2.F;
            float d = f1 - f2;
            if (d > 0) { return 1; }
            else if (d < 0) { return -1; }
            else { return 0; }
        }
        public static float MhtDistance(Point p1, Point p2)
        {
            return Math.Abs(p1.X - p2.X) + Math.Abs(p1.Y - p2.Y);
        }
        public static float EucDistance(Point p1, Point p2)
        {
            var dx = p1.X - p2.X;
            var dy = p1.Y - p2.Y;
            return MathF.Sqrt(dx * dx + dy * dy);
        }
    }
    public class AStarPointList : List<PointAStar>
    {
        public bool ContainsPoint(int x, int y)
        {
            foreach (var p in this) 
            {
                if(p.X == x && p.Y ==y)
                {
                    return true;
                }
            }
            return false;
        }
        public bool ContainsPoint(Point p)
        {
            return this.ContainsPoint(p.X, p.Y);
        }
        public PointAStar GetPoint(int x, int y)
        {
            foreach (var p in this)
            {
                if (p.X == x && p.Y == y)
                {
                    return p;
                }
            }
            return null;
        }
        public PointAStar RemovePoint(int x, int y)
        {
            int idx = -1;
            for (int i = 0; i < this.Count; i++)
            {
                if(this[i].X == x && this[i].Y ==y)
                {
                    idx = i;
                    break;
                }
            }
            if (idx == -1) 
            {
                return null;
            }
            else
            {
                var point = this[idx];
                this.RemoveAt(idx);
                return point;
            }
        }     
        public void SortByG()
        {
            this.Sort(PointAStar.CompareByG);
        }
        public void SortByF()
        {
            this.Sort(PointAStar.CompareByF);
        }
    }
    
    public class Path : List<Point>
    {


    }
}
